/**
  * GreenPois0n Cynanide - entry.S
  * Copyright (C) 2010 Chronic-Dev Team
  * Copyright (C) 2010 Joshua Hill
  * Copyright (C) 2010 Cyril Cattiaux
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
  * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 **/

#include "device.h"

.arm
.ltorg
.code 32
.global _start
_start:
	b		reset
	ldr	pc,	undefined_vector
	ldr	pc,	syscall_vector
	ldr	pc,	prefetch_abort_vector
	ldr	pc,	data_abort_vector
	ldr	pc,	reserved_vector
	ldr	pc,	irq_vector
	ldr	pc,	fiq_vector

reset_vector:		.word reset
undefined_vector:	.word halt
syscall_vector:		.word halt
prefetch_abort_vector:	.word halt
data_abort_vector:	.word halt
reserved_vector:	.word halt
irq_vector:		.word halt
fiq_vector:		.word halt

.pool
.set	new_loadaddr,		0x41000000
.set	new_payload_dest,	0x42F00000
.set	old_loadaddr,		0x09000000
.set	old_payload_dest,	0x0A000000

.code 32
@---------------------------------------------
reset:
	push	{r0-r12, lr}
	mov	r5, pc
	lsr	r5, #24
	cmp	r5, #0x42
	beq	relocated

	cmp	r5, #0x0A
	beq	relocated

	cmp	r5, #0x41
	beq	relocate_new
	
	cmp	r5, #0x09
	beq	relocate_old

	b	halt

relocate_new:
	ldr	r0, =new_loadaddr
	ldr	r1, =new_payload_dest
	b	relocate

relocate_old:
	ldr	r0, =old_loadaddr
	ldr	r1, =old_payload_dest
	b	relocate
	
relocate:
	mov	r4, r1
	bl	copy
	cmp	r5, #0x09
	bne	flush_new
	@bl	clear_dcache
	bl	clear_icache
	b	jump_main
flush_new:
	bl	clear_icache
	bl	flush_dcache

jump_main:
	blx	r4
	b	done

relocated:
	ldr	r0, [sp, #0x0]
	ldr	r1, [sp, #0x4]
	ldr	r2, [sp, #0x8]
	ldr	r3, [sp, #0xc]
	bl	main
	str r0, [sp, #0x0] @ push r0 return onto the stack to be returned

done:
	pop	{r0-r12, pc}

halt:
	b      halt

@---------------------------------------------
.code 32
copy:
	mov	r2, #0x00100000
copy_loop:
	ldr	r3, [r0], #4
	str	r3, [r1], #4
	subs	r2, r2, #4
	bne	copy_loop
	bx	lr

@---------------------------------------------
.code 32
clear_icache:
	mov	r0, #0
	mcr	p15, 0, r0, c7, c5
	mcr	p15, 0, r0, c7, c5, 4  @ Flush Prefetch Buffer
	nop
	nop
	nop
	nop
	bx	lr

@---------------------------------------------
.code 32
flush_dcache:
	mrc	p15, 0, r0, c1, c0, 1
	bx	lr

@---------------------------------------------
.code 32
clear_dcache:
    mov r0, #0
    mcr p15, 0, r0, c7, c10, 0  @ Clean Entire Data Cache
    mcr p15, 0, r0, c7, c10, 4  @ Data Synchronization Barrier
    bx lr

@---------------------------------------------
.end
